#include <Servo.h>
#include "functions.h"
const int trigPin = A1;//SD3 25
const int echoPin = A2;//SA3 26

double nowPaw=90,nowA1=90,nowA2=90,nowA3=90;
#define is_around(src,tgt) (tgt-1 <= src && src <= tgt+1)

long duration;
double lastDistance = 0;
double distance = 0; //cm

Servo s1,s2,s3,s4,s5,s6;

double a2[] = {49.3,48.7,48.7,48.7,48.7,48.7,48.7,49.3,49.8,49.8,50.4,51.0,52.1,52.7,53.8,55.0,56.1,57.3,58.4,60.2,61.9,63.6,65.9,68.7,71.0,74.5,79.0,86.5,10.9,13.2,14.9,16.6,18.9,21.2,24.1,26.4,30.4,33.8,39.5,48.7,8.0,12.0,17.8,28.1};
double a3[] = {107.1,105.4,103.7,102.0,100.2,98.5,96.2,94.5,92.2,90.0,87.6,85.4,82.5,80.2,77.3,74.5,71.0,68.2,64.7,61.3,57.3,53.3,48.7,43.5,38.4,31.5,24.0,10.3,75.1,71.6,68.8,64.2,60.2,55.6,51.0,45.3,39.5,32.1,23.5,6.9,40.7,33.2,24.1,7.4};

#define QUICK 10
#define SLOW 30

void setup() {
  Serial.begin(9600);
  Serial.println("hello");
  // Serial.println(sizeof(a2)/sizeof(double));
  // Serial.println(sizeof(a3)/sizeof(double));
  s2.attach(3); //第二个
  s5.attach(9); //第五个
  s3.attach(5); //第三个 反转
  s4.attach(6); //第四个 反
  s1.attach(7); //最底下
  s6.attach(8); //夹子 数字大合起来
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

char buf[64];
void loop() {
  s1.write(90);
  releaseBlock();
  Serial.println("Measuring...");
  do{
    measureDistance();
  } while(!is_around(distance,lastDistance));
  sprintf(buf,"distance: %d.%d angle: %d %d %d\n",(int)distance,(int)((distance-(int)distance)*10),(int)getA1(distance),(int)getA2(distance),(int)getA3(distance));
  if(distance > 0 && distance < 17.5) {
    Serial.print(buf);
    findBlock();
    catchBlock();
    turnToY();
    setA3(80+getA3(distance + 2));
    releaseBlock();
  } else {
    Serial.println("Failed. Please restart.");
  }
  while(1);
  delay(1500);
}

void measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  lastDistance = distance;
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
  delay(100);
}

void findBlock() {
  double d = distance + 2;
  setA1(getA1(d));
  setA2(90+getA2(d));
  setA3(80+getA3(d));
}
void releaseBlock() {
  setPaw(0);
  //delay(200);
}
void catchBlock() {
  setPaw(125);
  delay(300);
}

void turnToY() {
  turnTo(s1,90,160,SLOW);
  delay(300);
}

void fixedTurnTo(Servo s,double from,double to) {
  double div = (to-from)/6;
  to = from + 5 * div;
  div = (to-from)/10;
  int i;
  for(i=1;i<=10;i++) {
    s.write(from + div * i);
    delay((i/5+1)*50);
  }
}
void turnTo(Servo s,double from,double to,int speed) {
  double div = (to-from)/speed;
  int i;
  for(i=1;i<=speed;i++) {
    s.write(from + div * i);
    delay((i/(speed/2)+1)*30);
  }
}
void setPaw(int angle) {
  turnTo(s6,nowPaw,angle,QUICK);
  nowPaw = angle;
}

void setA1(int angle) {
  fixedTurnTo(s2,nowA1,angle);
  nowA1 = angle;
}
void setA2(int angle) {
  fixedTurnTo(s3,nowA2,angle);
  nowA2 = angle;
}
void setA3(int angle) {
  fixedTurnTo(s4,nowA3,angle);
  nowA3 = angle;
}
double getA1(double d) {
  if(d <= 13.5)  {
    return 60;
  } else {
    return 30;
  } 
}

double getA2(double d) {
  d *= 2;
  int index = (int)d;
  index += d-index > 0.5 ? 1 : 0;
  return a2[index];
}

double getA3(double d) {
  d *= 2;
  int index = (int)d;
  index += d-index > 0.5 ? 1 : 0;
  return a3[index];
}
